/**
 * @file /src/kinovaStatus.cpp
 *
 * @brief receive kinova status and parse it!
 *
 * @date May 2012
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include "../include/kinova_driver_qt/kinovaStatusHandler.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace kinova_driver_qt {

kinovaStatusHandler::kinovaStatusHandler( )
{

}

kinovaStatusHandler::~kinovaStatusHandler()
{

}

Status kinovaStatusHandler::parseStatus( QByteArray byteArray )
{
    QXmlStreamReader xml(byteArray);
    Status kinovaStatus;
    while(!xml.atEnd() &&
          !xml.hasError())
    {
            QXmlStreamReader::TokenType token = xml.readNext();
            if(token == QXmlStreamReader::StartDocument)
            {
                    continue;
            }
            if(token == QXmlStreamReader::StartElement)
            {
                if(xml.name() == "status")
                {
                    continue;
                }
                kinovaStatus.cartesian = this->parseCposition(xml);
                kinovaStatus.joints    = this->parseJoints(xml);
                kinovaStatus.fingers   = this->parseFingers(xml);
                kinovaStatus.isActive  = true;
                break;
            }
    }
    return kinovaStatus;
}

/*############################################################################*/
Cartesian kinovaStatusHandler::parseCposition(QXmlStreamReader &xml)
{
    Cartesian kinovaCartesian;
    if(xml.tokenType() != QXmlStreamReader::StartElement &&
       xml.name() == "cposition")
    {
        qDebug()<<"cposition error";
    }
    xml.readNext();
    while(!(xml.tokenType() == QXmlStreamReader::EndElement &&
            xml.name() == "cposition"))
    {
        if(xml.tokenType() == QXmlStreamReader::StartElement)
        {
            if(xml.name() == "position")
            {
                kinovaCartesian.position = parsePosition(xml);
            }
            if(xml.name() == "rotation")
            {
                kinovaCartesian.rotation = parseRotation(xml);
            }
        }
        xml.readNext();
    }
    return kinovaCartesian;
}
/*############################################################################*/
geometry_msgs::Point32 kinovaStatusHandler::parsePosition(QXmlStreamReader &xml)
{
    geometry_msgs::Point32 position;
    if(xml.tokenType() != QXmlStreamReader::StartElement &&
       xml.name() == "position")
    {
        qDebug()<<"position error";
    }
    xml.readNext();

    while(!(xml.tokenType() == QXmlStreamReader::EndElement &&
                    xml.name() == "position"))
    {
        if(xml.tokenType() == QXmlStreamReader::StartElement)
        {
                if(xml.name() == "x")
                {
                    position.x = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "y")
                {
                    position.y = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "z")
                {
                    position.z = this->getAddElementData(xml).toFloat();
                }
        }
        xml.readNext();
    }
    return position;
}
/*############################################################################*/
geometry_msgs::Point32 kinovaStatusHandler::parseRotation(QXmlStreamReader &xml)
{
    geometry_msgs::Point32 rotation;

    if(xml.tokenType() != QXmlStreamReader::StartElement &&
       xml.name() == "rotation")
    {
        qDebug()<<"rotation error";
    }
    xml.readNext();
    while(!(xml.tokenType() == QXmlStreamReader::EndElement &&
                    xml.name() == "rotation"))
    {
        if(xml.tokenType() == QXmlStreamReader::StartElement)
        {
                if(xml.name() == "theta_x")
                {
                        rotation.x = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "theta_y")
                {
                        rotation.y = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "theta_z")
                {
                        rotation.z = this->getAddElementData(xml).toFloat();
                }
        }
        xml.readNext();
    }
    return rotation;
}
/*############################################################################*/
Joints kinovaStatusHandler::parseJoints(QXmlStreamReader &xml)
{
    Joints joints;

    if(xml.tokenType() != QXmlStreamReader::StartElement &&
       xml.name() == "joints")
    {
        qDebug()<<"joints error";
    }
    xml.readNext();
    while(!(xml.tokenType() == QXmlStreamReader::EndElement &&
                    xml.name() == "joints"))
    {
        if(xml.tokenType() == QXmlStreamReader::StartElement)
        {
                if(xml.name() == "joint_1")
                {
                        joints.joint1 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "joint_2")
                {
                        joints.joint2 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "joint_3")
                {
                        joints.joint3 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "joint_4")
                {
                        joints.joint4 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "joint_5")
                {
                        joints.joint5 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "joint_6")
                {
                        joints.joint6 = this->getAddElementData(xml).toFloat();
                }
        }
        xml.readNext();
    }
    return joints;
}
/*############################################################################*/
Fingers kinovaStatusHandler::parseFingers(QXmlStreamReader &xml)
{
    Fingers fingers;

    if(xml.tokenType() != QXmlStreamReader::StartElement &&
       xml.name() == "fingers")
    {
        qDebug()<<"fingers error";
    }
    xml.readNext();
    while(!(xml.tokenType() == QXmlStreamReader::EndElement &&
                    xml.name() == "fingers"))
    {
        if(xml.tokenType() == QXmlStreamReader::StartElement)
        {
                if(xml.name() == "finger_1")
                {
                        fingers.finger1 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "finger_2")
                {
                        fingers.finger2 = this->getAddElementData(xml).toFloat();
                }
                if(xml.name() == "finger_3")
                {
                        fingers.finger3 = this->getAddElementData(xml).toFloat();
                }
        }
        xml.readNext();
    }
    return fingers;
}
/*############################################################################*/
QString kinovaStatusHandler::getAddElementData(QXmlStreamReader& xml)
{
    if(xml.tokenType() != QXmlStreamReader::StartElement)
    {
        return QString();
    }
    QString elementName = xml.name().toString();
    xml.readNext();
    if(xml.tokenType() != QXmlStreamReader::Characters)
    {
        return QString();
    }
    return xml.text().toString();
}
/*#########################################################################*/
}  // namespace kinova_driver_qt
